#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <sys/io.h>
#include "parport.h"

void debug_pport(uint32_t base)
{
    printf("data = %x\n", inb(base));
    printf("status = %x\n", inb(base+1));
    printf("control = %x\n", inb(base+2));
}

void write_control_port(uint32_t base, uint8_t value) {
    outb(value, base + 2);
}

uint8_t read_control_port(uint32_t base) {
    return inb(base + 2);
}

void write_epp_addr(uint32_t base, uint8_t data)
{
    outb(data, base + 3);
}

void write_epp_data(uint32_t base, uint8_t data)
{
    outb(data, base + 4);
}

uint8_t read_epp_data(uint32_t base)
{
    return inb(base + 4);
}

int wait_status1(uint32_t base) {
    uint8_t status;
    uint32_t i;

    for ( i = 0; i < 500 ; i++)
    {
        status = inb(base + 1 );
        if ( (status & 0x38) && !(status & 0x40) )
        {
            //printf("wait_status1 ok after %d reads\n", i);
            return 1;
        }
    }
    return 0;
}

int wait_status_ack(uint32_t base)
{
    uint8_t status;
    uint32_t i;

    for ( i = 0; i < 500 ; i++)
    {
        status = inb(base + 1 );

        if ( status & 0x40 )
        {
            //printf("wait_status_ack ok after %d reads\n", i);
            return 1;
        }
    }
    return 0;
}

int wait_status_ack_noerror(uint32_t base)
{
    uint8_t status;
    uint32_t i;

    for ( i = 0; i < 16 ; i++)
    {
        status = inb(base + 1 );

        if ( (status & 0x40) && !(status & 8) )
        {
            //printf("wait_status_ack_noerror ok after %d reads\n", i);
            return 1;
        }
    }
    return 0;
}

uint8_t read_nibble_from_status(uint32_t base)
{
    uint32_t i;
    uint8_t status;

    for ( i=0 ; i < 16 ; i++ )
    {
        status = inb(base + 1);
        if ( !(status & 0x40) )
        {
            //printf("read_nibble_from_status ok after %d reads\n", i);
            return status;
        }
    }
    return 0;
}


int read_nibble(uint32_t base, uint8_t* out)
{
  uint8_t high = 0;
  uint8_t low = 0;
  uint8_t nibble = 0;

  if ( wait_status_ack_noerror(base) )
  {
    write_control_port(base, 6);
    low = read_nibble_from_status(base);
    write_control_port(base, 4);
    
    if (wait_status_ack_noerror(base))
    {
        write_control_port(base, 6);
        high = read_nibble_from_status(base);
        write_control_port(base, 4);
        
        //XXX  
        if ( low & 8 )
          nibble = 1;
        if ( low & 0x10 )
          nibble |= 2;
        if (low & 0x20 )
          nibble |= 4;
        if ( !(low & 0x80) )
          nibble |= 8;
        if ( high & 8 )
          nibble |= 0x10;
        if ( high & 0x10 )
          nibble |= 0x20;
        if ( high & 0x20 )
          nibble |= 0x40;
        if ( !(high & 0x80) )
          nibble |= 0x80;
          
        if ( out != NULL)
            *out = nibble;
        return 1;
    }
  }
  return 0;
}

bool xfer_data_epp(uint32_t base, uint32_t reg_num, uint8_t* out)
{
    uint8_t data;
    uint32_t i;

    if ( reg_num < 0x40 )
    {
        write_epp_addr(base, 75);
        for ( i=0 ; i < 500 ; i++ )
        {
            data = read_epp_data(base);
            *out = data;
            if ( data & 0x80 )
            {
                *out = data & 0x7F;
                return 1;
            }
        }
    }
    return 0;
}

int read_register_epp(uint32_t base, uint32_t addr, uint8_t* out)
{
    write_epp_addr(base, addr);
    *out = read_epp_data(base);
    return xfer_data_epp(base, addr, out);
}

int write_register_epp(uint32_t base, uint32_t addr, uint8_t data)
{
    write_epp_addr(base, addr);
    write_epp_data(base, data);
    return xfer_data_epp(base, addr, &data);
}

